#ifndef __ZEUS_S2_DRIVER_H__
#define __ZEUS_S2_DRIVER_H__

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
#include "can-ctrl.h"
#include "zeus_s2_driver/controlcan.h"
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <dynamic_reconfigure/server.h>
#include <tf/tf.h>
#include "sensor_msgs/JointState.h"
#include "zeus_s2_msgs/Feedback.h"
#include "zeus_s2_msgs/wheel_state.h"
#include "zeus_s2_msgs/motor_pid_vol.h"
#include "zeus_s2_msgs/joy_control.h"
#include <zeus_s2_driver/PID_reconfigConfig.h> 


/*使用高配版nvidia-xavier主机，自带can通信*/
//#define NVIDIA_XAVIER    1

#define MAX_LINEAR  1.57        //m/s
#define MAX_ANGULAR  0.7853     //red/s

#define GROUP_INT_BYTE(X,N)     ((X[N+3]<<24)+(X[N+2]<<16)+(X[N+1]<<8)+X[N])
#define MOTOR_ID(X,N)           ((X[N]<<8 | X[N+1])>>5)

#define PID_INTEGRAL_UP     0.8             //积分上限
#define PID_SCALE           1.0             //PID缩放系数
#define PID_TEMP            1.5

#define MAX                 1910.0          //32767
#define MIN                 -1910.0         //-32768

#define PI                  3.1415926

#define MOTOR_DRV_MAX       4

enum _enum_DirveID{

    DRIVER_OPERATION_MODE = 1,
    DRIVER_TIMEOUT_DETECTION = 2,
    DRIVER_PARKING_MODE = 3,
    DRIVER_SWITCHING_MODE = 0,

    DIRVEE_ID_SYSTEM_STATE_ACK = 0x151,
	DIRVEE_ID_MOVE_CONTROL_ACK = 0x131,
	DIRVEE_ID_MOVE_CONTROL = 0x130,
	DIRVEE_ID_ONE_MOTOR_ACK = 0x201,
	DIRVEE_ID_TWO_MOTOR_ACK = 0x202,
	DIRVEE_ID_THREE_MOTOR_ACK = 0x203,
	DIRVEE_ID_FOUR_MOTOR_ACK = 0x204,
	DIRVEE_ID_LAMPLIGHT_CONTROL = 0x140,		//lamplight
	DIRVEE_ID_LAMPLIGHT_CONTROL_ACK = 0x141,

    DRIVER_ID_ONE_POSE_ACK = 0X601,
    DRIVER_ID_TWO_POSE_ACK = 0X602,
    DRIVER_ID_THREE_POSE_ACK = 0X603,
    DRIVER_ID_FOUR_POSE_ACK = 0X604,

    DRIVER_ID_ONE_CONTROL = 0x101,
    DRIVER_ID_TWO_CONTROL = 0x102,
    DRIVER_ID_THREE_CONTROL = 0x103,
    DRIVER_ID_FOUR_CONTROL = 0x104,

    DRIVER_ID_MODE_ACK = 0X700,
};

enum  _enum_timeManagement{
    TIME_DRIVER_MODE=0,
    TIME_SUM, 
};

struct  DRIVE_DATA
{
    uint8_t carBodyStateAck;
    int8_t  modeControlAck;
    uint16_t batteryVoltageAck;
    uint16_t faultMessageAck;

    int16_t movementSpeedAck;
    int16_t rotationalSpeedAck;
	
    uint8_t controlMode;
    uint8_t faultClearance;
    int8_t  percentLinear;
    int8_t  percentAngular;
   
    uint16_t oneMotorElectricCurrentAck;
    int16_t oneMotorRorateSpeedAck;
    int8_t  oneMotorTemperatureAck;
   
    uint16_t twoMotorElectricCurrentAck;
    int16_t twoMotorRorateSpeedAck;
    int8_t  twoMotorTemperatureAck;

    uint16_t threeMotorElectricCurrentAck;
    int16_t threeMotorRorateSpeedAck;
    int8_t  threeMotorTemperatureAck;

    uint16_t fourMotorElectricCurrentAck;
    int16_t fourMotorRorateSpeedAck;
    int8_t  fourMotorTemperatureAck;

    uint8_t lightControlEnableSign;
    uint8_t bodyLightPattern;

    uint8_t currentLightControlEnableSignAck;
    uint8_t bodyLightPatternAck;
} ;

class DriverPub
{
    public:
        DriverPub();
        ~DriverPub();
        void loop();
    private:
        bool initRobot(void);
        void receive_func(void);
        void send_speed_callback(void);
        void handle_speed_data(uint16_t number);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void cmd_joy_callback(const zeus_s2_msgs::joy_control::ConstPtr& msg);
        void enable_motor_driver(uint16_t driver_id );
        void disable_motor_driver(uint16_t driver_id );
        void clear_ALM_motor_driver(uint16_t driver_id );
        void Set_motor_torque_mode(uint16_t driver_id );
        void Set_torque_value_to_motor(uint16_t driver_id ,uint16_t value );

#ifdef  NVIDIA_XAVIER
        int step=0;
	    char tx_buf[8] = {0, 0x1E, 0, 0x03, 0, 1, 0, 0};
	    char rx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
        struct can_frame can_id;
#else
        VCI_BOARD_INFO pInfo;//用来获取设备信息。
        VCI_BOARD_INFO dev_list [50];
#endif
        void updateParam(int id, uint8_t *data);
        void Calculate_LeftAndRight_Meter(void *p);
        bool mode_switch_judge(void *handle,uint8_t mode,bool isnot);
        uint8_t get_recvFlag(void);
        int16_t motor_velocityPidCtl(int motor_id,float target_speed,float current_speed);
        void dynamic_reconfig_callback(zeus_s2_driver::PID_reconfigConfig &config,uint32_t level);
        void motor_driver_check(void);

        void Set_motor_speed_mode(uint16_t driver_id );
        void Set_speed_value_to_motor(uint16_t driver_id ,uint16_t value );
        uint64_t my_xTaskGetTickCount(void);
        void time_setMana(unsigned char time_id);
        unsigned char time_checkHandle(unsigned char time_id, unsigned short time_ms);
        void mode_switching_state_machine(uint8_t state, bool isnot);
      
        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
         ros::Subscriber cmd_joy_moment_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_feedback_;
        zeus_s2_msgs::Feedback  left_right_wheel_state_;
        ros::Publisher speed_pid_;                              //发布测试pid话题
        zeus_s2_msgs::motor_pid_vol  motor_pid_[4];             //调试pid参数类型
        ros::Timer timer1;                                      //ros的定时器

        std::string code_version_;
        std::string port_name_;

        std::string odom_frame_;
        std::string base_frame_;

        bool recv_flag_;
        bool start_flag_;
        bool publish_odom_transform_;
        bool joy_flag_=0;
        geometry_msgs::Twist current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
        tf2_ros::TransformBroadcaster br_;
        zeus_s2_msgs::joy_control   ps3_control_;

        int                         driver_count;

        int                         fdcom;
        int                         baud_rate_;
        unsigned char               buffer_data[100];

        int         ret;
        ros::Time   now_[4];
        ros::Time   last_time_[4];

        double      wheel_track_4wd_;
        double      wheel_track_;
        double      encoder_resolution_4wd_;
        double      ticks_per_meter_;                //脉冲数/m

        double      linear_correction_factor_;
        double      angular_correction_factor_;
        double      gear_reduction_4wd_;             //减速比
        double      wheel_diameter_4wd_;
        double      start_rotation_limit_w_;
        double      delta_time_[4];
        double      delta_xy_ave_;
        double      delta_th_;
        double      v_linear_;
        double      v_angular_;
        double      delta_x_;
        double      delta_y_;

        double      accumulation_x_;
        double      accumulation_y_;
        double      accumulation_th_;
        double      accumulation_left_;
        double      accumulation_left_1_;
        double      accumulation_right_;
        double      accumulation_right_1_;

        double      delta_left_;
        double      delta_right_;                    //里程差值 单位 m
        double      delta_left_1_;
        double      delta_right_1_;                  //里程差值 单位 m
        int32_t     rev_left_;
        int32_t     rev_left_1_;
        int32_t     rev_right_;       
        int32_t     rev_right_1_;
        int         cur_left_;                  //存放脉冲计数值
        int         cur_left_1_; 
        int         cur_right_;
        int         cur_right_1_;
        double      encoder_low_wrap_;          //解决编码器溢出
        double      encoder_high_wrap_;
        int         l_wheel_mult_;              //多圈计数
        int         l_wheel_mult_1_;            //多圈计数
        int         r_wheel_mult_;
        int         r_wheel_mult_1_;
        int         encoder_min_;
        int         encoder_max_;   

        int32_t     line_speed_percent;
        int32_t     angle_speed_percent;

        uint8_t     queueFlag;                  //用于通信检测标志
        uint8_t     can_recv_flag;

        float       motor_kp[4];
        float       motor_ki[4];
        float       motor_kd[4];
        int         ele_currVol;
        float       speed_l;
        float       speed_r;
        short       error;

        float       left_des;
        float       right_des;
        bool        reset_vol;
        bool        mode_selection;
        int         pid_wheel_;
        float       left;
        float       right;

        float       motorEleVol[4]={0};         //驱动器反馈电流值
        int16_t     motorPreSpeed[4] = {0};     //电机的实际速度
        int16_t     motorActSpeed[4] = {0};     //电机的实际速度
        int16_t     motorTemperature[4] = {0};  //电机温度
        int         driverFaultCode[4] = {0};   //驱动器故障码
        uint8_t     rev_flag;                   //接收驱动器信息并处理标志
        uint64_t    count = 0;                  //根据频率定时判断驱动器通信问题
        float       bias[4] = {0};
        float       bias_last[4] = {0};
        float       bias_integral[4] = {0};
        uint8_t     mode_rev_flag = 0;          //用于判断模式切换是否成功
        bool        mode_isnot = 0;
        uint8_t     switching_mode = 0;
        uint64_t    timeArray[2] = {0};         //用于计时数组
        uint8_t     state_temp_  =0;
};

#endif //__ZEUS_S2_DRIVER_H__



